/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : attitude_ctrl.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月24日
  ******************************************************************************
  */
/* USER CODE END Header */

#include "attitude_ctrl.hpp"

#define PID_NUM		6
PID pid[PID_NUM];
PID *pidRolRate=&pid[0],*pidPitRate=&pid[1],*pidYawRate=&pid[2];
PID *pidRol=&pid[3],*pidPit=&pid[4],*pidYaw=&pid[5];

ATTITUDE_CTRL::ATTITUDE_CTRL()
{
	pidRolRate->Pid_Set_Para(1.5f, 1.5f, 0.02f, 0.5f, 1000, 4*M_PI, M_PI);
	pidPitRate->Pid_Set_Para(1.5f, 1.5f, 0.02f, 0.5f, 1000, 4*M_PI, M_PI);
	pidYawRate->Pid_Set_Para(2.0f, 2.0f, 0.0f, 0.0f, 1000, 4*M_PI, M_PI);
	pidRol->Pid_Set_Para(35.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
	pidPit->Pid_Set_Para(35.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
	pidYaw->Pid_Set_Para(35.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
}

ATTITUDE_CTRL::~ATTITUDE_CTRL()
{

}


void ATTITUDE_CTRL::pid_Para_Update()
{
	xQueuePeek(queuePID, &pid_para, 0);
	for(uint8_t i=0;i<PID_NUM;i++)
	{
		pid[i].Pid_Set_Para(pid_para.kp[i], pid_para.ki[i], pid_para.kd[i]);
	}
}

void ATTITUDE_CTRL::Attitude_Ctrl_Init()
{
	pid_Para_Update_Time = 0;
	THROTTLE = 0;

	osDelay(300);

	xQueuePeek(queuePID, &pid_para, 0);
	for(uint8_t i=0;i<PID_NUM;i++)
	{
		pid_para.kp[i] = pid[i].Pid_Get_Kp();
		pid_para.ki[i] = pid[i].Pid_Get_Ki();
		pid_para.kd[i] = pid[i].Pid_Get_Kd();
	}
	xQueueOverwrite(queuePID,&pid_para);

}

void ATTITUDE_CTRL::Attitude_ctrl_run()
{

	getTimer_us(&startTimer);
	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);

	xQueuePeek(queueRCCommand, &rcCommand, 0);
//	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
	xQueuePeek(queueMahonyAhrs, &mahonyAhrs_Ang, 0);
	xQueuePeek(queueInloopControl, &inloop_control, 0);
	xQueuePeek(queueHeight, &height, 0);

	pid_Para_Update_Time++;
	if(pid_Para_Update_Time>50)		//50个控制周期更新一次PID参数
	{
		pid_Para_Update();
		pid_Para_Update_Time = 0;
	}
	if(inloop_control.thrust_PWM < 400.0f||height.height<15.0f)
	{
		for(u8 i=0; i<PID_NUM; i++)
			pid[i].Pid_Reset();
	}

	//------------姿态角控制-------------------//
	SetAng[0] = inloop_control.setAng[0];
	SetAng[1] = inloop_control.setAng[1];
	Z_OUT = inloop_control.thrust_PWM;

	if(rcCommand.Key[3]<1)
		HoldYaw = mahonyAhrs_Ang.Ang[2]*R2D;

	//姿态角反馈值
	FeedBackAng[0] = mahonyAhrs_Ang.Ang[0]*R2D;
	FeedBackAng[1] = mahonyAhrs_Ang.Ang[1]*R2D;

	pidRol->Pid_Controller(SetAng[0], FeedBackAng[0],0.001f,0);
	pidPit->Pid_Controller(SetAng[1], FeedBackAng[1],0.001f,0);
	float sinR, cosR, sinP, cosP;

	if(rcCommand.dAng[2]>-15.0f&&rcCommand.dAng[2]<15.0f)
	{
		SetAng[2] = HoldYaw;//锁尾，航向控角度
		FeedBackAng[2] = mahonyAhrs_Ang.Ang[2]*R2D;
		pidYaw->Pid_Controller(SetAng[2], FeedBackAng[2],0.001f,4);
		//欧拉角速度转pqr
		sinR = sinf(mahonyAhrs_Ang.Ang[0]);
		cosR = cosf(mahonyAhrs_Ang.Ang[0]);
		sinP = sinf(mahonyAhrs_Ang.Ang[1]);
		cosP = cosf(mahonyAhrs_Ang.Ang[1]);
		SetDAng[0] =  pidRol->get_PIDOutput()      - pidYaw->get_PIDOutput()*sinP;
		SetDAng[1] =  pidPit->get_PIDOutput()*cosR + pidYaw->get_PIDOutput()*cosP*sinR;
		SetDAng[2] = -pidPit->get_PIDOutput()*sinR + pidYaw->get_PIDOutput()*cosP*cosR;
	}
	else
	{
		HoldYaw = mahonyAhrs_Ang.Ang[2]*R2D;
		SetDAng[0] =  pidRol->get_PIDOutput();
		SetDAng[1] =  pidPit->get_PIDOutput();
		SetDAng[2] = rcCommand.dAng[2];//遥控器控航向角速度
	}


	FeedBackDAng[0] = gyro_filter.gyro_filter[0]*R2D;
	FeedBackDAng[1] = gyro_filter.gyro_filter[1]*R2D;
	FeedBackDAng[2] = gyro_filter.gyro_filter[2]*R2D;
	pidRolRate->Pid_Controller(SetDAng[0],FeedBackDAng[0],0.001f,1);
	pidPitRate->Pid_Controller(SetDAng[1],FeedBackDAng[1],0.001f,1);
	pidYawRate->Pid_Controller(SetDAng[2],FeedBackDAng[2],0.001f,1);

	ROL_OUT = pidRolRate->get_PIDOutput();
	PIT_OUT = pidPitRate->get_PIDOutput();
	YAW_OUT = pidYawRate->get_PIDOutput();

	//------------姿态角控制-------------------//
	motor_Output();

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

void ATTITUDE_CTRL::motor_Output()
{
	if(Z_OUT <80)
	{
		#ifdef QUADX	//X字型的四轴飞行器
			motorPWM.PWM[0] = PWM_IDLE; //FRONT_L 前左电机
			motorPWM.PWM[1] = PWM_IDLE; //REAR_L  后左电机
			motorPWM.PWM[2] = PWM_IDLE; //FRONT_R 前右电机
			motorPWM.PWM[3] = PWM_IDLE; //REAR_R  后右电机
		#endif
	}
	else
	{
		#define MIX(X,Y,Z) THROTTLE + FOREWARD + Z_OUT + ROL_OUT*(X) + PIT_OUT*(Y) + YAW_OUT*(Z)

		#ifdef QUADP	//十字型的四轴飞行器
			motorPWM.PWM[4] = MIX( 0,+1,+1); //FRONT 前面电机
			motorPWM.PWM[5] = MIX(+1, 0,-1); //LEFT	 左边电机
			motorPWM.PWM[6] = MIX( 0,-1,+1); //REAR	 后尾电机
			motorPWM.PWM[7] = MIX(-1, 0,-1); //RIGHT 右边电机
		#endif
		#ifdef QUADX	//X字型的四轴飞行器
			motorPWM.PWM[3] = MIX(+1,+1,+1); //FRONT_L 前左电机
			motorPWM.PWM[2] = MIX(+1,-1,-1); //REAR_L  后左电机
			motorPWM.PWM[0] = MIX(-1,+1,-1); //FRONT_R 前右电机
			motorPWM.PWM[1] = MIX(-1,-1,+1); //REAR_R  后右电机
		#endif
		#ifdef OCTAP	//十字型的八轴飞行器
			const float rt2 = 1.414213562373f;
			motorPWM.PWM[4] = MIX(   0,  +2,-1); //1号电机
			motorPWM.PWM[5] = MIX(-rt2, rt2,+1); //2号电机
			motorPWM.PWM[6] = MIX(  -2,   0,-1); //3号电机
			motorPWM.PWM[7] = MIX(-rt2,-rt2,+1); //4号电机
			motorPWM.PWM[0] = MIX(   0,  -2,-1); //5号电机
			motorPWM.PWM[1] = MIX( rt2,-rt2,+1); //6号电机
			motorPWM.PWM[2] = MIX(  +2,   0,-1); //7号电机
			motorPWM.PWM[3] = MIX( rt2, rt2,+1); //8号电机
		#endif
	}
	xQueueOverwrite(queueMotorPWM,&motorPWM);
}


ATTITUDE_CTRL attitude_ctrl;
extern "C" void attitudeCtrl_main(void *argument)
{
	attitude_ctrl.Attitude_Ctrl_Init();
	for(;;)
	{
		osSemaphoreAcquire(semAttitudeCtrl,0xffffffff);
		attitude_ctrl.Attitude_ctrl_run();

	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
